/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

void p3p1(double *x,double *q1,double *q2,double *q3,double *p1,double *p2,double *p3, int *num);
void p3p2(double *x,double *q1,double *q2,double *q3,double *p1,double *p2,double *p3);

void calc_p3pRt(double *q,double *R,double *t, double *pointsA,double  *pointsB);
/** calculates 6D pose given 3 3D-point matches
* @param *q [out] rotation represented in quaternion 4 element array 
* @param *R [out] rotation matrix in 9 element array
* @param *t [out] translation represented in 3 element array 
* @param *pointsA [in] 3x3 array of 3D-landmark coordinates in body coordinates
* @param *pointsB [in] 3x3 array of 3D-landmark coordinates in world coordinates
*/
void calc_p3pRt1(double *q,double *R,double *t, double *pointsA,double  *pointsB);
void p3p2dev(double *,double *, double *, double *, double *,double *,double  *,int);
void p3p2dev1(double *,double *, double *, double *, double *,double *,double  *,int);
void p3p2devt(double *dev,double *res, double *R, double *t, double *cal,double *pointsA,double  *pointsB, int num_points,double thres);
void p3p2devt1(double *dev,double *res, double *R, double *t, double *cal,double *pointsA,double  *pointsB, int num_points,double thres);
void p3p2cnt(int *count, double *R, double *t, double *cal,double *pointsA,double  *pointsB, int numPoints,double thres);

void calc_p3p(double *q,double *t,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points);
/** calculates pose in presence of outliers using 3-point RANSAC with LSmed given landmark 3D coordinates and their image 2D coordinates
* @param *q [out] rotation represented in quaternion 4 element array 
* @param *t [out] translation represented in 3 element array 
* @param *cal [in]  intrinsic camera parameters focal length and image center represented in 3 element array 
* @param *res [in]  helper array of num_points elements
* @param *devmin [out]  the score for the best pose( parameters q and t)
* @param *pointsA [in] 2xnum_points array of 2D-image coordinates of candidate matches to landmarks 
* @param *pointsB [in] 3xnum_points array of 3D-landmark coordinates of candidate matches
* @param num_points [in] number of points with candidate matches
*/
void ransac_calc_p3p(double *q,double *t,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points, double thres);
/** 
* calculates pose in presence of outliers using 3-point RANSAC given landmark 3D coordinates and their image 2D coordinates
* @param *q [out] rotation represented in quaternion 4 element array 
* @param *t [out] translation represented in 3 element array 
* @param *cal [in]  intrinsic camera parameters focal length and image center represented in 3 element array 
* @param *res [in]  helper array of num_points elements
* @param *devmin [out]  the score for the best pose( parameters q and t)
* @param *pointsA [in] 2xnum_points array of 2D-image coordinates of candidate matches to landmarks 
* @param *pointsB [in] 3xnum_points array of 3D-landmark coordinates of candidate matches
* @param num_points [in] number of points with candidate matches
*/
void ransac_calc_p3p1(double *q,double *t,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points, double thres);
void ransac_calc_p3p_v(double *q,double *t,double *oq,double *ot,double dq, double dt,double *cal,double *res, 
double *devmin,double *pointsA,double  *pointsB, int num_points, double thres);
void ransac_calc_p3p_vl(double *q,double *t,double *oq,double *ot,double dq, double dt,double *cal,double *res, double *devmin,double *pointsA,double  *pointsB, int num_points, double thres,double  scale);

void calc_rot(double *q, double *X,double *pointsA,double  *pointsB, int num_points, double *V, double *U,double *S);
void esse_funt(double *ssd,double *JJ, double *Je,double *v,double *pts1,double *pts2,double th,int N);
void esse_calct(double *v,double *pts1,double *pts2,double *lambda, double th, int N);
/**/
void proj3D_funt(double *ssd,double *JJ,double *Je,double *v,double *pts,double *X,int *mask,double th,int N);
void proj3D_funt1(double *ssd,double *JJ,double *Je,double *v,const double *pts,const double *X,const double th,const int N);
void proj3D_funtl1(double *ssd,double *JJ,double *Je,double *v,double *v0,double r,double scale,const double *pts,const double *X,const double th,const int N);
void pose_calct(double *v,double *pts1,double *pts2,int *mask,double *lambda, double th, int N);
/** refine pose using levenberg-marquardt minimasing trucated ssd error given initial estimate and 2D-3D point matches
* @param *v [in/out] 10 element array (quaternion representation of rotation and 3D translation three calibration parameters)   
* @param *pts1 [in] 2xN array of 2D-image coordinates of candidate matches to landmarks 
* @param *pts2 [in] 3xmax(mask) array of 3D-landmark coordinates of candidate matches
* @param *mask [in/out] 1xN array  mask[i] is the index pts2[(mask[i]-1)*3] for candidate 3D point of 2D point pts1[2*i], when mask[i] is 0 point does not have candidate match   
* @param *lambda [in]  lambda in levenber-marquardt
* @param th [in] threshold distance used in to truncate error
* @param N [in] number of points with candidate matches 
*/
void pose_calct1(double *v,const double *pts1,const double *pts2,double *lambda, const double th,const int N);
/** refine pose using levenberg-marquardt minimasing trucated ssd error given initial estimate and 2D-3D point matches
* @param *v [in/out] 10 element array (quaternion representation of rotation and 3D translation three calibration parameters)   
* @param *pts1 [in] 2xN array of 2D-image coordinates of candidate matches to landmarks 
* @param *pts2 [in] 3xmax(mask) array of 3D-landmark coordinates of candidate matches
* @param *lambda [in]  lambda in levenber-marquardt
* @param th [in] threshold distance used in to truncate error
* @param N [in] number of points with candidate matches 
*/
void pose_calctl1(double *v,double *v0,double r,double S,const double *pts1,const double *pts2,double *lambda,const double th,const int N);
void triangulate2(double *X,double *Xpos,double *v,double *pts1,double *pts2,int *ind2,int *indX,int *mask,double th,int *kk,int N);
void robust_triangulate(double *X,double *Xpos,double *v,double *pts1,double *pts2,int *ind2,int *indX,int *mask,double th,int *kk,int N);
void robust_triangulate2(double *X,double *Xpos,double *v,double *p1,double *p2,double *pts1,double *pts2,int *ind2,int *indX,int *mask,double th,int *kk,int N);
/** triangulate 3D-coordinates of landmarks given image-coordinates of points in two images 
* @param *X [out] 3xkk array of calculated landmark coordinates 
* @param *Xpos [in] 7 element array (quaternion representation of rotation and 3D translation) containing pose where the first image was taken  
* @param *v [in] 7 element array (quaternion representation of rotation and 3D translation) containing pose where the second image was taken  
* @param *p1 [in]  first cameras intrinsic camera parameters focal length and image center represented in 3 element array  
* @param *p2 [in]  second cameras intrinsic camera parameters focal length and image center represented in 3 element array  
* @param *pts1 [in] 2xN array of 2D-image coordinates of points in first image 
* @param *pts2 [in] 2xN array of 2D-image coordinates of points in second image 
* @param *ind2 [in] 1xN array containing the index of tracked landmark that the corresponding points are of.
* @param *indX [out] at least 1xkk array containing the index of tracked landmark that the corresponding triangulated landmark coordiantes are of.
* @param *mask [int] 1xN array only points with mask[i]==0 are candidates for triangulation, points with failed triangulation get mask[i] set to -1
* @param th [in] threshold distance from correspoding epipolar line. When distance smaller than th triangulation is calculated for the point pair
* @param N [in] number of points with candidate matches 
*/
void robust_triangulate1(double *X,double *Xpos,double *v,double *pts1,double *pts2,int *ind2,int *indX,int *mask,double th,int *kk,int N);
void triangulate(double *X,double *v,double *pts1,double *pts2,double th,int N);
/** triangulate 3D-coordinates of landmarks given image-coordinates of points in two images 
* @param *X [out] 3xkk array of calculated landmark coordinates 
* @param *Xpos [in] 7 element array (quaternion representation of rotation and 3D translation) containing pose where the first image was taken  
* @param *v [in] 7 element array (quaternion representation of rotation and 3D translation) containing pose where the second image was taken  
* @param *pts1 [in] 2xN array of 2D-image coordinates of points in first image 
* @param *pts2 [in] 2xN array of 2D-image coordinates of points in second image 
* @param th [in] threshold distance from correspoding epipolar line. When distance smaller than th triangulation is calculated for the point pair
* @param N [in] number of points with candidate matches 
*/
void relative_transf(double *v1,double *v2,double *v3);
void relative_transf1(double *v1,double *v2,double *v3);
void proj3D_err(double *er,double *v,double *pts,double *X,int *maski,double th,int *count,int *kk,int N);
void proj3D_err1(double *er,double *v,double *pts,double *X,int *maski,double th,int *count,int *kk,int N);
void calcdev(double *dev,double *res, double *F, double *pointsA, double  *pointsB,int num_points);
void calcdevh(double *dev,double *res, double *H, double *pointsA,double  *pointsB, int num_points);
void calcdevp(double *dev,double *res, double *H, double *pointsA,double  *pointsB, int num_points,double *p);
void decompose_E(double *R1,double *R2,double *t,double *E);
void calc_funda(double *F, double *res, double *devmin, double *X,double *pointsA,double  *pointsB, int num_points);
void test_chirality(double *v,double *pts1,double *pts2,int num_points, int *pos);
void calc_esse(double *q,double *t,double *pointsA,double *pointsB, double *X, double *res, double *dev, int num_points);
/* */
void compose_F(double *F,double *q,double *t,double *pl,double *pr);
void compose_E(double *E,double *q,double *t);
void ransac_calc_rot(double *F, double *res, double *devmin, double *pointsA,double  *pointsB, int num_points);
void ransac_calc_rot_p(double *F, double *res, double *devmin, double *pointsA,double  *pointsB, double *p,int num_points);

void ransac_calc_funda(double *F, double *devmin,double *pointsA,double  *pointsB, double thres, int num_points);
void ransac_calc_funda7(double *F, double *devmin,double *pointsA,double  *pointsB, double thres, int num_points);
/**/
void calcdevt(double *dev, double *F, double *pointsA, double  *pointsB, double thres, int num_points);
void motion3D_update(double *v1,double *v2,double *v3);
void motion3D_update1(double *v1,double *v2,double *v3);
void inv_rigid_motion(double *vout, double *vin);
void move3D(double *v,double *X1,double *X2,int N);
void ransac_calc_esse(double *q,double *t,double *pointsA,double *pointsB,  double *res, double *dev, double thres, int num_points);
void ransac_calc_esse7(double *q,double *t,double *pointsA,double *pointsB,  double *res, double *dev, double thres, int num_points);
/* 
*/
void find_zero_det_F(double *E,double *F1,double *F2);
void proj3D(double *v,double *p,double *pts,double *X,int N);
void v2H(double *P, double *v,double *n, double *p);
